#include "pmw3901.hpp"
#include <glog/logging.h>

using namespace std;

// #define pmw3901_DEBUG
// #define pmw3901_DEBUG_RAW

/******************************************************************************
 *
 * @brief:     pmw3901类实现
 *
 ******************************************************************************/
pmw3901::pmw3901(spi_v1 *spi,std::string gpioChip,uint32_t dePinOffset)
    : _spi(spi),
      _gpioChip(gpioChip),
      _dePinOffset(dePinOffset),
      _unread_x_move(0),
      _unread_y_move(0)
{
    _init_flag = YJ_NULL;
    if (!spi)
    {
        LOG(INFO) << "input pra is null";
        goto end;
    }
    if (spi->is_open() != YJ_OK)
        goto end;

    if(_init_reset_pin() != YJ_OK)
        goto end;

    _init_flag = YJ_NOT_INIT;
    LOG(INFO) << "pmw3901 init ok!";
    return;
end:
    _init_flag = YJ_ERROR;
    LOG(ERROR) << "pmw3901 init fail!";
    return;
}
pmw3901::~pmw3901()
{
    _deinit_reset_pin();
}

yj_return pmw3901::init()
{
    uint8_t retry_time = 3;
    int cmd_pause_time = 1;
    uint8_t skip_flag = 0;
    uint8_t C1, C2;

    //    spi->CHIP_SPI_open_debug();


    std::array<uint8_t, 1> reg;
    std::array<uint8_t, 2> buff;

    { // power_up 上电

        // reset_pin->CHIP_OUTPUT_PIN_write(chip_pin_level_LOW);
        // rt_thread_mdelay(1000);
        // reset_pin->CHIP_OUTPUT_PIN_write(chip_pin_level_HIGH);
        // rt_thread_mdelay(1000);

        // reset
        _reset();

        // read id
        reg[0] = pmw3901_reg_PRODUCT_ID;
        if (_spi->transfer_and_recv(reg.data(), 1, buff.data(), 1) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
        if (buff[0] != 0x49)
        {
            LOG(ERROR) << "read pmw3901 id 0x" << std::hex << std::setw(2) << (int)buff[0] << " != 0x49 , connect error showed";
            goto end;
        }

        reg[0] = pmw3901_reg_MOTION;
        if (_spi->transfer_and_recv(reg.data(), 1, buff.data(), 1) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        reg[0] = pmw3901_reg_DELTA_X_L;
        if (_spi->transfer_and_recv(reg.data(), 1, buff.data(), 1) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        reg[0] = pmw3901_reg_DELTA_X_H;
        if (_spi->transfer_and_recv(reg.data(), 1, buff.data(), 1) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        reg[0] = pmw3901_reg_DELTA_Y_L;
        if (_spi->transfer_and_recv(reg.data(), 1, buff.data(), 1) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        reg[0] = pmw3901_reg_DELTA_Y_H;
        if (_spi->transfer_and_recv(reg.data(), 1, buff.data(), 1) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
    }

    // set_optimum_performance 性能优化
    {

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x55;
        buff[1] = 0x01;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x50;
        buff[1] = 0x07;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x0E;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        while (retry_time-- && buff[1] != 0x08)
        {
            buff[0] = 0x80 | 0x43;
            buff[1] = 0x10;
            if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

            buff[0] = 0x47;
            buff[1] = 0x00;
            if (_spi->transfer_and_recv(buff.data(), 1, buff.data() + 1, 1) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
        }

        if (buff[1] != 0x08)
            goto end;

        buff[0] = 0x67;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 1, buff.data() + 1, 1) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
        if ((buff[1] & 0x80) == 0x80)
        {
            buff[0] = 0x80 | 0x48;
            buff[1] = 0x04;
            if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
        }
        else
        {
            buff[0] = 0x80 | 0x48;
            buff[1] = 0x02;
            if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
        }

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x51;
        buff[1] = 0x7B;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x50;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x55;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x0E;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x73;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 1, buff.data() + 1, 1) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
        if (buff[1])
        {
            LOG(INFO) << "pmw3901 0x73 read != 0x00 skip";
            skip_flag = 1;
        }
        else
        {
            LOG(INFO) << "pmw3901 0x73 read == 0x00 no skip";
            skip_flag = 0;
        }

        if (skip_flag == 0)
        {
            buff[0] = 0x70;
            buff[1] = 0x00;
            if (_spi->transfer_and_recv(buff.data(), 1, buff.data() + 1, 1) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
            if (buff[1] <= 28)
                C1 = buff[1] + 14;
            else
                C1 = buff[1] + 11;
            if (C1 > 0x3F)
                C1 = 0x3F;

            buff[0] = 0x71;
            buff[1] = 0x00;
            if (_spi->transfer_and_recv(buff.data(), 1, buff.data() + 1, 1) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
            C2 = buff[1] * 45 / 100;

            buff[0] = 0x80 | 0x7F;
            buff[1] = 0x00;
            if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

            buff[0] = 0x80 | 0x61;
            buff[1] = 0xAD;
            if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

            buff[0] = 0x80 | 0x51;
            buff[1] = 0x70;
            if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

            buff[0] = 0x80 | 0x7F;
            buff[1] = 0x0E;
            if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

            buff[0] = 0x80 | 0x70;
            buff[1] = C1;
            if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

            buff[0] = 0x80 | 0x71;
            buff[1] = C2;
            if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
                goto end;
            std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
        }

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x61;
        buff[1] = 0xAD;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x03;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x40;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x05;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x41;
        buff[1] = 0xB3;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x43;
        buff[1] = 0xF1;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x45;
        buff[1] = 0x14;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x5B;
        buff[1] = 0x32;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x5F;
        buff[1] = 0x34;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7B;
        buff[1] = 0x08;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x06;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x44;
        buff[1] = 0x1B;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x40;
        buff[1] = 0xBF;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x4E;
        buff[1] = 0x3F;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x08;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x65;
        buff[1] = 0x20;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x6A;
        buff[1] = 0x18;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x09;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x4F;
        buff[1] = 0xAF;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x5F;
        buff[1] = 0x40;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x48;
        buff[1] = 0x80;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x49;
        buff[1] = 0x80;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x57;
        buff[1] = 0x77;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x60;
        buff[1] = 0x78;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x61;
        buff[1] = 0x78;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x62;
        buff[1] = 0x08;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x63;
        buff[1] = 0x50;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x0A;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x45;
        buff[1] = 0x60;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x4D;
        buff[1] = 0x11;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x55;
        buff[1] = 0x80;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x74;
        buff[1] = 0x1F;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x75;
        buff[1] = 0x1F;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x4A;
        buff[1] = 0x78;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x4B;
        buff[1] = 0x78;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x44;
        buff[1] = 0x08;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x64;
        buff[1] = 0xFF;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x65;
        buff[1] = 0x1F;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x14;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x65;
        buff[1] = 0x67;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x66;
        buff[1] = 0x08;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x63;
        buff[1] = 0x70;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x15;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x48;
        buff[1] = 0x48;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x07;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x41;
        buff[1] = 0x0D;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x43;
        buff[1] = 0x14;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x4B;
        buff[1] = 0x0E;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x45;
        buff[1] = 0x0F;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x44;
        buff[1] = 0x42;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x4C;
        buff[1] = 0x80;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x10;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x5B;
        buff[1] = 0x02;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x07;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x40;
        buff[1] = 0x41;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x70;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        std::this_thread::sleep_for(std::chrono::milliseconds(20));

        buff[0] = 0x80 | 0x32;
        buff[1] = 0x44;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x07;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x40;
        buff[1] = 0x40;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x06;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x62;
        buff[1] = 0xF0;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x63;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x0D;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x48;
        buff[1] = 0xC0;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x6F;
        buff[1] = 0xD5;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x7F;
        buff[1] = 0x00;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x5B;
        buff[1] = 0xA0;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x4E;
        buff[1] = 0xA8;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x5A;
        buff[1] = 0x50;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));

        buff[0] = 0x80 | 0x40;
        buff[1] = 0x80;
        if (_spi->transfer_and_recv(buff.data(), 2, nullptr, 0) != YJ_OK)
            goto end;
        std::this_thread::sleep_for(std::chrono::milliseconds(cmd_pause_time));
    }
    LOG(INFO) << "pmw3901 optimum performance ok!";
    _init_flag = YJ_OK;
    return YJ_OK;
end:
    LOG(ERROR) << "pmw3901 optimum performance fail!";
    _init_flag = YJ_ERROR;
    return YJ_ERROR;
}

yj_return pmw3901::burst_read_motion(pmw3901_motion_data_st &motion)
{
    uint8_t addr = pmw3901_reg_MOTION_BURST;
    uint8_t buff[12] = {};
    if (_init_flag != YJ_OK)
        goto end;

    if (_spi->transfer_and_recv((uint8_t *)&addr, 1, (uint8_t *)buff, 12) != YJ_OK)
        goto end;
    if ((buff[0] & (1 << 7)) != 0)
    {
        if ((buff[6] < 0x19) && (buff[10] == 0x1F))
        {
            LOG(WARNING) << "pmw3901 detect move but data can't trust";
            motion.flag = YJ_ERROR; /* success return 0 */
        }
        motion.delta_x = (int16_t)(((uint16_t)buff[3] << 8) | buff[2]);
        motion.delta_y = (int16_t)(((uint16_t)buff[5] << 8) | buff[4]);
        motion.flag = YJ_OK;

        // LOG (INFO) << "delta_x:" << motion.delta_x << " delta_y:" << motion.delta_y;
    }
    else
    {
        motion.flag = YJ_ERROR;
    }

    //    rt_kprintf("\r\n");
    //    for(i = 0 ; i < 12;i++)
    //    {
    //        rt_kprintf("%d :<0x%.2X> ",i,buff[i]);
    //    }
    //    rt_kprintf("\r\n");
    //    LOG_I("pmw3901 brust read motion finish!");
    return YJ_OK;
end:
    LOG(ERROR) << "pmw3901 brust read motion fail!";
    return YJ_ERROR;
}

yj_return pmw3901::read_move(pmw3901_move_data_st &move_data)
{
    uint32_t temp;
    std::unique_lock<std::mutex> lock(_data_lock, std::defer_lock);
    if (_init_flag != YJ_OK)
        goto end;

    lock.lock();
    move_data.x_move = _unread_x_move;
    move_data.y_move = _unread_y_move;
    _unread_x_move = 0;
    _unread_y_move = 0;
    lock.unlock();

    return YJ_OK;
end:
    return YJ_ERROR;
}
yj_return pmw3901::count_shift_with_height(double height)
{
    pmw3901_motion_data_st motion;
    std::unique_lock<std::mutex> lock(_data_lock, std::defer_lock);
    double resolution_cpi;
    double x, y;

    if (_init_flag != YJ_OK)
        goto end;

    if (burst_read_motion(motion) != YJ_OK)
        goto end;
    if (motion.flag != YJ_OK)
        goto cant_trust;

    // 放置在此处以便于清除错误数据后再退出
    if (height == 0)
        goto end;

    resolution_cpi = 11.914f / (height * 25.4) * 1000;

    //    LOG_D("cx %d, cy %d",motion.delta_x,motion.delta_y);
    x = motion.delta_x / resolution_cpi; // mm
    y = motion.delta_y / resolution_cpi; // mm

    lock.lock();
    _unread_x_move += x;
    _unread_y_move += y;
    lock.unlock();

    return YJ_OK;
cant_trust:
    return YJ_ERROR;
end:
    LOG(ERROR) << "pmw3901 count shift fail!";
    return YJ_ERROR;
}

yj_return pmw3901::read_id()
{
    uint8_t buff[2] = {0};
    // read id
    buff[0] = pmw3901_reg_PRODUCT_ID;
    if (_spi->transfer_and_recv(buff, 1, buff + 1, 1) != YJ_OK)
        goto end;

    if (buff[0] != 0x49)
    {
        LOG(ERROR) << "read pmw3901 id 0x%.2X != 0x49 , connect error showed", buff[0];
        goto end;
    }
    return YJ_OK;
end:
    return YJ_ERROR;
}

yj_return pmw3901::_init_reset_pin()
{
    if (_init_flag == YJ_OK)
    {
        LOG(WARNING) << " already inited, skip...";
        return YJ_OK;
    }
    _gpioChip_p = gpiod_chip_open(_gpioChip.c_str());
    if (!_gpioChip_p)
    {
        LOG(ERROR) << "[gpioChip: " << _gpioChip << "]"
                   << "[dePinOffset: " << _dePinOffset << "]"
                   << " Failed to open GPIO chip";
        return YJ_ERROR;
    }

    // 获取GPIO线
    _deLine = gpiod_chip_get_line(_gpioChip_p, _dePinOffset);
    if (!_deLine)
    {
        LOG(ERROR) << "[gpioChip: " << _gpioChip << "]"
                   << "[dePinOffset: " << _dePinOffset << "]"
                   << " Failed to get GPIO line";
        gpiod_chip_close(_gpioChip_p);
        _gpioChip_p = nullptr;
        return YJ_ERROR;
    }

    // 检查当前线路状态
    if (gpiod_line_is_used(_deLine))
    {
        LOG(ERROR) << "[gpioChip: " << _gpioChip << "]"
                   << "[dePinOffset: " << _dePinOffset << "]"
                   << " GPIO line is currently in use,try to release it ";

        _deLine = gpiod_chip_get_line(_gpioChip_p, _dePinOffset);

        gpiod_line_release(_deLine);
        // gpiod_chip_close(_gpioChip_p);
        // _gpioChip_p = nullptr;
        // return false;

        // std::cout << "Debug: GPIO line is currently in use, attempting to release..." << std::endl;
        // gpiod_line_release(m_deLine);
    }

    std::string pin_name = "pmw3901_rst";
    // 配置GPIO为输出模式，初始状态为低电平（接收模式）
    struct gpiod_line_request_config config = {
        .consumer = pin_name.c_str(),
        .request_type = GPIOD_LINE_REQUEST_DIRECTION_OUTPUT,
        .flags = 0,
    };

    if (gpiod_line_request(_deLine, &config, 0) < 0)
    {
        LOG(ERROR) << "[rs485_v3][gpioChip: " << _gpioChip << "]"
                   << "[dePinOffset: " << _dePinOffset << "]"
                   << " Failed to request GPIO line ";
        // m_lastError = "Failed to request GPIO line: " + std::string(strerror(errno));
        // std::cout << "Debug: " << m_lastError << std::endl;
        gpiod_chip_close(_gpioChip_p);
        _gpioChip_p = nullptr;
        return YJ_ERROR;
    }

    // 确认GPIO设置成功
    int current_value = gpiod_line_get_value(_deLine);
    // LOG(INFO) << "[rs485_v3][gpioChip: " << _gpioChip << "]"
    //            << "[dePinOffset: " << _dePinOffset << "]"
    //            << "Initial GPIO value: " << current_value;

    return YJ_OK;
}

yj_return pmw3901::_deinit_reset_pin()
{
    // std::cout << "Debug: Releasing DE/RE pin..." << std::endl;
    if (_deLine)
    {
        gpiod_line_release(_deLine);
        _deLine = nullptr;
    }
    if (_gpioChip_p)
    {
        gpiod_chip_close(_gpioChip_p);
        _gpioChip_p = nullptr;
    }
    return YJ_OK;
}

// 发送设置允许设置后延时以保证发送电平稳定，但接收设置在发送完成后要求以最快速度执行
void pmw3901::_reset()
{
    gpiod_line_set_value(_deLine, 0);
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    gpiod_line_set_value(_deLine, 1);
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}

